#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Encoder_Interface.h"
#include "TIM.h"
#include "PWM.h"
#include "Uart.h"
#include "DMA_UART1.h"
#include <stdio.h>
#include "Rp.h"
#include "Key.h"


extern int16_t rpm;


int main(void)
{
    
    RCC_HSEConfig(RCC_HSE_ON);
    
    while(RCC_WaitForHSEStartUp()==ERROR);
    
    RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9);
    
    RCC_PLLCmd(ENABLE);
    
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
    
 
    
	/*模块初始化*/
	OLED_Init();		//OLED初始化
	
    Key_Init();

    
    init_uart_with_dma();
    
    init_dma_for_uart1();
    
    init_tim();
    
    

    init_encoder_interface_in_tim3();
    
    init_my_pwm_in_tim2();
    
    start_pwm();
    
    initRp();
    
   
	while (1)
	{
        
        
        /*
        static uint32_t count = 0;
        count++;
        sprintf(buf,"count:%d",count);
        OLED_ShowString(1,1,buf);
        */
       
        /*
        sprintf(buf,"tim3_cnt:%d",TIM_GetCounter(TIM3));
        OLED_ShowString(1,1,buf);
        
        sprintf(buf,"adc_DR:%d",ADC_GetConversionValue(ADC1));
        OLED_ShowString(2,1,buf);
        */

        
        
	}
}
/**
    adc 的 dma 传输完成中断已被关闭
*/
void DMA1_Channel1_IRQHandler(void){
    if(DMA_GetITStatus(DMA1_IT_TC1)==SET){
        /*
        static char buf[20];
        
        
        sprintf(buf,"rp1:%d %d", get_rp_val(0),k1);
        OLED_ShowString(1,1,buf);
        
        sprintf(buf,"rp2:%d %d", get_rp_val(1),k2);
        OLED_ShowString(2,1,buf);
        
        sprintf(buf,"rp3:%d %d", get_rp_val(2),k3);
        OLED_ShowString(3,1,buf);
        
        sprintf(buf,"rp4:%d %d", get_rp_val(3),k4);
        OLED_ShowString(4,1,buf);
        */
        //uint16_t duty = get_rp_val(0)*2.44;//*10000/4096;
        
        //TIM_SetCompare1(TIM2,duty);
        
        DMA_ClearITPendingBit(DMA1_IT_TC1);
    }
}

uint16_t k1=1,k2=0,k3=0,k4=0;

void EXTI15_10_IRQHandler(void){
    
    if(EXTI_GetITStatus(EXTI_Line10)==SET){
        if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_10)==Bit_RESET){
            //K1 被按下
            if(k1==0){
                k1 = 1;
                start_pwm();
            }else{
                k1 = 0;
                stop_pwm();
            }
        }
        EXTI_ClearITPendingBit(EXTI_Line10);
    }else if(EXTI_GetITStatus(EXTI_Line11)==SET){
        if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11)==Bit_RESET){
            //K2 被按下
            k2++;
        }else if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_11)==Bit_RESET){
            //K3 被按下
            k3++;
        }
        
        EXTI_ClearITPendingBit(EXTI_Line11);
    }
    else if(EXTI_GetITStatus(EXTI_Line12)==SET){
        if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_12)==Bit_RESET){
            //K4 被按下
            k4++;
        }
        EXTI_ClearITPendingBit(EXTI_Line12);
    }
    
}

#define MAX_RPS 23

static float error_sum = 0;
static float last_error = 0;
/**
    PID 三个参数从某种意义上分别代表了调控的快(p比例)、准(i积分)、稳(d微分)
    
    位置式PID
        缺点：
            1、积分饱和：积分项的累加求和如果在Ki处于0时，误差会被不断的累加，
                当某次调参时，设置Ki>0时，会有一个很明显的抖动
                解决：当Ki==0时，对积分球和不进行累加
    增量式PID
        优点：
            1、不存在位置式PID的积分饱和问题
                因为out项的《输出限幅》 兼具《积分限幅》的功能
            2、把最终pid函数的数据结果out直接简单粗暴的限幅到电机输出的范围，不用考虑其他
            3、方便自动控制和手动控制平滑切换
                把Kp、Ki、Kd全都设置为0，手动修改out值
        缺点:
            Kp一项，具体是Kp(error0 - error1)  会引入历史信息
            如果每次存在微小的error变化，那么Kp项很难自动调节，因为依赖了error1，甚至有积分项的特性
            加上Ki项才能缓解这个问题
            所以增量式PID调节，不建议Ki项为0
            
*/
float pid(float error,float Kp,float Ki,float Kd,float dt){
    
   
    /*
    二、积分分离（应用于位置式pid控制电机位置场景）
        PD 控制器的缺点：
            1、target 与 current 之间总是存在误差无法消除
                这是因为误差过小导致Kp项无法对其进行修正
            2、用手小幅拨动转盘，发现能轻易改变电机位置（电机输出太小）
        引入积分项（Ki）后能解决问题一，但是会额外引入下列问题：
            1、由于电机定位控制在out达到target时，
                不需要额外的 积分项（Ki）来抵抗摩檫力，
                所以会导致积分项（Ki）输出之前累积过大值，出现过调
        解决方案：当误差值大于阈值的时候关闭Ki，反之则开启Ki
    */
    
    float result = Kp * error 
        +  Ki * error_sum * dt
        +  Kd * (error- last_error)/dt;
    
    error_sum += error;
    
    /*
    一、积分限幅
    这里的积分上限设置为：
    当pid输出最大 rps ，且只有 Ki项不为0 时
    积分的和
    */
    float max_sum =  Ki * error_sum;
    
    if(error_sum > 200){
        /**
        为解决位置式pid 积分饱和问题
        这里采用积分限幅功能
        幅值 200 由观察上位机一般情况下最大的 error_sum 来确定
        */
        error_sum = 200;
    }else if(error_sum < -200){
        error_sum = -200;
    }
    last_error = error;
    
    return result;
}



void TIM4_IRQHandler(void){
    
    if(TIM_GetITStatus(TIM4,TIM_IT_Update)){
        
        static uint16_t lastCount = 0;
        // 这里应该改为读取之后将其清零，这样就能每次都读取编码器的增量
        uint16_t currentCount = TIM_GetCounter(TIM3);
        /*
            定时越快，读取值的频率越快，但是测量精度会下降
            定时越慢，测量精度越高，但是获取值的频率就必须降下来
        
            注意这里实现正反转的话，涉及到的变量类型必须是有符号的
        */
        int16_t delta_count = currentCount - lastCount;
        
        float pulse_per_rotation = 4 * 11 *9.27666; // 脉冲每转
        // 因为这个方法40ms触发一次，所以要乘以25Hz得到转/秒
        // 1/0.04 = 25
        float current_rps = delta_count * 25 / pulse_per_rotation;
        
        
        
        static char buf[20];
        
        // 电机目标速度：范围 0 ~ 23 rps
        float target_rps = get_rp_val(0) * 2 * MAX_RPS  / 4096  - MAX_RPS;

        
        // 当前比较好的参数 p=1.7 i=3.5 d=?
        float Kp = get_rp_val(1)/4096.0 * 2.5;
        
        float Ki = get_rp_val(2)/4096.0 * 3.5;
        float Kd = get_rp_val(3)/4096.0 * 0.05;
        
        float error = target_rps - current_rps;
        
        float output_rps = pid(error,Kp,Ki,Kd,0.04);
        
        uint16_t output_duty = set_speed(output_rps,MAX_RPS);
        
        sprintf(buf,"tar:%+3.1f %4d", target_rps,currentCount);
        
        OLED_ShowString(1,1,buf);
        
        static uint16_t location=0;
        
        
        location+=delta_count;
        
        
        
        sprintf(buf,"out:%03.f-%.1f",output_rps, Kp);
        OLED_ShowString(2,1,buf);
        
        sprintf(buf,"cur:%03.1f-%.1f", current_rps,Ki);
        OLED_ShowString(3,1,buf);
        
        sprintf(buf,"dut:%03d-%.3f", output_duty,Kd);
        OLED_ShowString(4,1,buf);
        
        lastCount = currentCount;
        
        uint8_t * str  = getBuffer();

    
        int length = sprintf((char *)str,"%3.1f,%3.1f,%3.1f,%03.2f,%03.2f,%03.2f,%03.2f,%03.2f,%03.2f\r\n",
            target_rps,output_rps,current_rps,
            Kp*10,Ki*10,Kd*10,
            error,
            error_sum,
            last_error
        );  
        
        send(length);
        
        TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
    }
}

